#include <ximera/physics/ode/space.hpp>
#include <ximera/physics/ode/entity.hpp>
#include <ximera/physics/ode/world.hpp>

namespace ximera { namespace physics { namespace ode
{
    Space::Space(World* world, SpaceType type)
        : _world(world), 
          _type(type)
    {
        XIMERA_ASSERT_DEF(world);

        switch (_type)
        {
        case SpaceType::Simple:
            _spaceID = dSimpleSpaceCreate(0);
            break;

        case SpaceType::Hash:
            _spaceID = dHashSpaceCreate(0);
            break;

        case SpaceType::QuadTree:
            //oops!: dQuadTreeSpaceCreate necesita parametros extra
            //el prototipo es: dSpaceID dQuadTreeSpaceCreate (dSpaceID space, dVector3 Center, dVector3 Extents, int Depth);
            //quedemos con hash space por ahora...

            //_spaceID = dQuadTreeSpaceCreate(0, ...);
            //_spaceID = dHashSpaceCreate(0);
            XIMERA_THROW(NotImplementedException, L"Quadtree spaces have not been implemented");
            break;
        }
    }

    Space::~Space()
    {
        removeAllEntities();
        removeAllJoints();

        if (_spaceID)
            dSpaceDestroy(_spaceID);
    }

    SpaceType Space::type() const
    {
        return _type;
    }

    IWorld* Space::world() const
    {
        return _world;
    }

    IEntity* Space::createEntity(EntityType type)
    {
        Entity* entity = new Entity(this, type);
        addEntity(entity);

        return entity;
    }

    UInt32 Space::entityCount() const
    {
        return (UInt32)_entities.size();
    }

    IEntity* Space::entity(UInt32 index)
    {
        return _entities[index];
    }

    void Space::addEntity(IEntity* entity)
    {
        XIMERA_ASSERT(entity, XIMERA_THROW(InvalidParamException, L"Parameter 'entity' must not be null"));

        if (std::find(_entities.begin(), _entities.end(), entity) != _entities.end())
            return;

        Entity* odeEntity = static_cast<Entity*>(entity);

        if (odeEntity->odeSpace())
            odeEntity->odeSpace()->removeEntity(odeEntity);

        odeEntity->attach(this);

        _entities.push_back(entity);
    }

    void Space::removeEntity(IEntity* entity)
    {
        XIMERA_ASSERT(entity, XIMERA_THROW(InvalidParamException, L"Parameter 'entity' must not be null"));

        Entity* odeEntity = static_cast<Entity*>(entity);

        if (odeEntity->odeSpace() != this)
            return;

        odeEntity->detach();

        _entities.erase(std::remove(_entities.begin(), _entities.end(), entity), _entities.end());
    }

    void Space::removeAllEntities()
    {
        for (EntityContainer::iterator it = _entities.begin(); it != _entities.end(); ++it)
        {
            Entity* entity = static_cast<Entity*>(*it);
            entity->detach();

            delete entity;
        }

        _entities.clear();
    }

    IJoint* Space::createJoint(JointType type)
    {
        return 0;
    }

    UInt32 Space::jointCount() const
    {
        return (UInt32)_joints.size();
    }

    IJoint* Space::joint(UInt32 index)
    {
        return _joints[index];
    }

    void Space::addJoint(IJoint* joint)
    {
        _joints.push_back(joint);
    }

    void Space::removeJoint(IJoint* joint)
    {
        _joints.erase(std::remove(_joints.begin(), _joints.end(), joint), _joints.end());
    }

    void Space::removeAllJoints()
    {
        for (JointContainer::iterator it = _joints.begin(); it != _joints.end(); ++it)
        {
            IJoint* joint = static_cast<IJoint*>(*it);

            delete joint;
        }

        _joints.clear();
    }
}}}
